#!/usr/bin/env python


import os, sys, yaml
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

import rospy
import tf2_ros
from geometry_msgs.msg import Twist,PoseStamped, Transform,PoseArray,Pose,PoseWithCovarianceStamped
import numpy as np
from ros_numpy import numpify,msgify
from nav_msgs.msg import Path
from sensor_msgs.msg import PointCloud2
from tf.transformations import quaternion_from_euler,euler_matrix
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import MultiDOFJointTrajectory,MultiDOFJointTrajectoryPoint
import torch
import torch.optim as optim
import matplotlib.pyplot as plt

def extend_AB(A,B,n_horizon,dt):
    A_ext=A
    for i in range(1,n_horizon):
        A_ext=torch.vstack([A_ext,torch.matrix_power(A,i+1)])

    B_ext=torch.zeros([2*n_horizon,2*n_horizon])  
    for r in range(n_horizon):
        for c in range(n_horizon):
            if r>=c:
                B_ext[2*r:2*r+2,2*c:2*c+2]=torch.matrix_power(A,r-c)@B
    B_ext=B_ext*dt
    
    return A_ext,B_ext

def hw():
    print('hello world!!!bbb')